#include "Steering.h"

Vector3D Steering::Normalize(Vector3D v3)
{
	v3.CalculateMagnitude();
	Vector3D normalized(v3.getX() / v3.getMagnitude(), v3.getY() / v3.getMagnitude(), v3.getZ() / v3.getMagnitude());

	return normalized;
}

//this should take it a pointer to whatever velocity it should be modifying
//for example, if Seek() uses Sleep(), then Sleep will modify that velocity to be 0
//not just the velocity in general like it is now
void Steering::Sleep()
{
	pOwnedObject->setVelocity(0, 0, 0);
}

//Seek sets the velocity vector of MovingObject it's attached to
void Steering::Seek()
{
	//checks to see if the object has reached it's target
	if(pOwnedObject->isAtTarget())
	{
		//if it has, stop moving
		Sleep();
	}
	else
	{
		//sets the desired angle it should turn in order to face the correct way
		//for seek, this is looking directly at it's target
		/*float angle = (atan2(pOwnedObject->getTarget().getY() - pOwnedObject->getPosition().getY(),  
						(pOwnedObject->getTarget().getX() - pOwnedObject->getPosition().getX()) * -1));*/

		float angle = atan2(pOwnedObject->getTarget().getY() - pOwnedObject->getPosition().getY(),
							(-1 * pOwnedObject->getTarget().getX()) - (-1 * pOwnedObject->getPosition().getX()));

		Vector3D DesiredVelocity = Normalize(pOwnedObject->getTarget() - pOwnedObject->getPosition()) * pOwnedObject->getSpeed();

		//pOwnedObject->setSeekVelocity(DesiredVelocity);
		pOwnedObject->setVelocity(DesiredVelocity);
		pOwnedObject->setDesiredAngle(angle);
	}
}
void Steering::Flee()
{
	Vector3D DesiredVelocity = Normalize(pOwnedObject->getPosition() - pOwnedObject->getTarget()) * pOwnedObject->getSpeed();

	//this should look something like this after I finish modifying code
	//pOwnedObject->setFleeVelocity(DesiredVelocity);
	pOwnedObject->setVelocity(DesiredVelocity);
	//for now, this is fine. I'll change it after I've modified the structure if you want
}

void Steering::Evade()
{
	//Length/MyMax+HisSpeed
	float LookAheadTime = pOwnedObject->getPosition().DistanceTo(pOwnedObject->getTarget()) /(pOwnedObject->getSpeed()+0); //Where zero is position's speed
	Vector3D DesiredVelocity = Normalize(pOwnedObject->getPosition() + (pOwnedObject->getVelocity()*LookAheadTime));
	
	pOwnedObject->setVelocity(DesiredVelocity);
}